
package JET.physics.engine;

import JET.geom.Vec2d;
import JET.physics.Mass;
import JET.physics.MaterialPair;
import JET.physics.SBCollision;
import JET.physics.Wall;
import JET.physics.bodies.SpringBody;

/**
 *
 * @author raos
 */
public class SBCollider {

    private static final SBCollision infoAway = new SBCollision();
    private static final SBCollision infoSame = new SBCollision();
    private static final Vec2d hitPt = new Vec2d();
    private static final Vec2d norm = new Vec2d();
    private static final Vec2d bVel = new Vec2d();
    private static final Vec2d relVel = new Vec2d();
    private static final Vec2d helpVecA = new Vec2d();
    private static final Vec2d helpVecB = new Vec2d();
    private static final Vec2d numV = new Vec2d();
    private static final Vec2d tangent = new Vec2d();
    private static final double[] edgeD = new double[1];
    private static final Vec2d tempVec = new Vec2d();

    public static double mPenetrationThreshold = 25;
    public static int mPenetrationCount=0;

    public static void collisionCheck_SpringBodies(JETContext c) {
        SpringBody sb1 = null,
                   sb2 = null;

        for (int i=0; i<c.springBodyList.size()-1; i++)
            for (int j=i+1; j<c.springBodyList.size(); j++)
            {
                sb1 = c.springBodyList.get(i);
                sb2 = c.springBodyList.get(j);

                MaterialPair matP = c.materials[sb1.getMaterial()][sb2.getMaterial()];
                if (matP.collide) {
                    collide(c, sb1, sb2 );
                    collide(c, sb2, sb1 );
                }
            }
    }

    public static void collide(JETContext c, SpringBody sb1, SpringBody sb2) {
        for(int i=0; i<sb1.getBorder().getWallsCount(); i++ ) {
            Mass mass = sb1.getBorder().getWall(i).getM1();

            if ( !sb2.getBoundingArea().contains(mass.getPosition()) )
                continue;

            if ( !sb2.getBorder().contains( mass.getPosition()) )
                continue;

            Vec2d ptNorm = sb1.getBorder().getMassNormal(mass);

            double closestAway = 1000000.;
            double closestSame = 1000000.;

            infoAway.clear();
            infoAway.body1 = sb1;
            infoAway.bodyApm = mass;
            infoAway.body2 = sb2;

            infoSame.clear();
            infoSame.body1 = sb1;
            infoSame.bodyApm = mass;
            infoSame.body2 = sb2;

            boolean found = false;

            hitPt.become(Vec2d.zero);
            norm.become(Vec2d.zero);

            //double[] edgeD = new double[1];
            edgeD[0] = 0;
            for (int j=0; j<sb2.getBorder().getWallsCount(); j++)
            {
                Wall wall = sb2.getBorder().getWall(j);

                // test against this edge.
                double dist = sb2.getBorder().getClosestPointOnEdgeSquared(mass.getPosition(), wall, hitPt, norm, edgeD);

                double dot = ptNorm.dot(norm);

                if (dot <= 0.0f)
                {
                    if (dist < closestAway)
                    {
                        closestAway = dist;
                        infoAway.bodyBwall = wall;
                        infoAway.edgeD = edgeD[0];
                        infoAway.hitPt.become(hitPt);
                        infoAway.norm.become(norm);
                        infoAway.penetration = dist;
                        found = true;
                    }
                }
                else
                {
                    if (dist < closestSame)
                    {
                        closestSame = dist;
                        infoSame.bodyBwall = wall;
                        infoSame.edgeD = edgeD[0];
                        infoSame.hitPt.become(hitPt);
                        infoSame.norm.become(norm);
                        infoSame.penetration = dist;
                    }
                }
            }

            // we've checked all edges on BodyB.  add the collision info to the stack.
            if ((found) && (closestAway > mPenetrationThreshold) && (closestSame < closestAway))
            {
                infoSame.penetration = Math.sqrt(infoSame.penetration);
                //sbColInfoList.add(infoSame);
                c.sbColInfoList.get(c.sbColInfoCount++).set(infoSame);
            }
            else
            {
                infoAway.penetration = Math.sqrt(infoAway.penetration);
                //sbColInfoList.add(infoAway);
                c.sbColInfoList.get(c.sbColInfoCount++).set(infoAway);
            }

            if (c.sbColInfoCount>=c.sbColInfoList.size())
                for(int abcd=0; abcd<50; abcd++)
                    c.sbColInfoList.add(new SBCollision());
        }
    }

    public static void handleCollisions(JETContext c) {
        //for(SBCollision info : sbColInfoList) {
        for(int i=0; i<c.sbColInfoCount; i++) {
            SBCollision info = c.sbColInfoList.get(i);

            SpringBody sb1 = (SpringBody) info.body1;
            SpringBody sb2 = (SpringBody) info.body2;

            hitPt.become(Vec2d.zero);
            norm.become(Vec2d.zero);
            bVel.become(Vec2d.zero);
            relVel.become(Vec2d.zero);
            helpVecA.become(Vec2d.zero);
            helpVecB.become(Vec2d.zero);
            numV.become(Vec2d.zero);
            tangent.become(Vec2d.zero);

            Mass A = info.bodyApm;
            Mass B1 = info.bodyBwall.getM1();
            Mass B2 = info.bodyBwall.getM2();

            // velocity changes as a result of collision.
//            bVel=  B1.getVelocity().getAdded(B2.getVelocity()).getScaled(.5);
//            relVel = A.getVelocity().getSubbed(bVel);
            bVel.become(B1.getVelocity());
            bVel.add(B2.getVelocity());
            bVel.scale(.5);

            relVel.become(A.getVelocity());
            relVel.sub(bVel);

            //double relDot = relVel.dot(info.norm);
            double relDot = info.norm.dot(relVel);

            if (info.penetration > mPenetrationThreshold)
            {
                //Console.WriteLine("penetration above Penetration Threshold!!  penetration={0}  threshold={1} difference={2}",
                //    info.penetration, mPenetrationThreshold, info.penetration-mPenetrationThreshold);
                //printf("handleCollisions - penetration above threshold! threshold:%f penetration:%f diff:%f\n",
                //	   mPenetrationThreshold, info.penetration, info.penetration - mPenetrationThreshold);
                mPenetrationCount++;
                continue;
            }

            double b1inf = 1.0 - info.edgeD;
            double b2inf = info.edgeD;

            //double b2MassSum = ((B1.mass == 0.0f) || (B2.mass == 0.0f)) ? 0.0f : (B1.mass + B2.mass);
            double b2MassSum = (!B1.isMoveable() || !B2.isMoveable()) ? 0.0f : (B1.mass + B2.mass);

            double massSum = A.mass + b2MassSum;

            double Amove;
            double Bmove;
            if (A.mass == 0.0f)
            {
                Amove = 0.0f;
                Bmove = (info.penetration) + 0.001f;
            }
            else if (b2MassSum == 0.0f)
            {
                Amove = (info.penetration) + 0.001f;
                Bmove = 0.0f;
            }
            else
            {
                Amove = (info.penetration * (b2MassSum / massSum));
                Bmove = (info.penetration * (A.mass / massSum));
            }

            double B1move = Bmove * b1inf;
            double B2move = Bmove * b2inf;

            //printf("handleCollisions - Amove:%f B1move:%f B2move:%f\n",
            //	   Amove, B1move, B2move)
            if (A.isMoveable())
            {
                helpVecA.become(info.norm);
                helpVecA.scale(Amove);
                //A.getPosition().add(info.norm.getScaled(Amove));
                A.getPosition().add(helpVecA);
            }

            if (B1.isMoveable())
            {
                helpVecA.become(info.norm);
                helpVecA.scale(B1move);
                //B1.getPosition().sub(info.norm.getScaled(B1move));
                B1.getPosition().sub(helpVecA);
            }

            if (B2.isMoveable())
            {
                helpVecA.become(info.norm);
                helpVecA.scale(B2move);
                //B2.getPosition().sub(info.norm.getScaled(B2move));
                B2.getPosition().sub(helpVecA);
            }

            double AinvMass = (!A.isMoveable()) ? 0.0 : 1.0 / A.mass;
            double BinvMass = (b2MassSum == 0.0) ? 0.0 : 1.0 / b2MassSum;

            double jDenom = AinvMass + BinvMass;
            //double elas = 1.0 + mMaterialPairs[(info.bodyA->getMaterial() * mMaterialCount) + info.bodyB->getMaterial()].Elasticity;
            double elas = 1.0 + c.materials[sb1.getMaterial()][sb2.getMaterial()].elasticity;

//            Vec2d numV = relVel.getScaled(elas);
            numV.become(relVel);
            numV.scale(elas);

            double jNumerator = numV.dot(info.norm);
            jNumerator = -jNumerator;

            double j = jNumerator / jDenom;

            //Vec2d tangent = info.norm.getPerp();
            tangent.become(info.norm);
            tangent.makePerp();

            //double friction = mMaterialPairs[(info.bodyA->getMaterial() * mMaterialCount) + info.bodyB->getMaterial()].Friction;
            double friction = c.materials[sb1.getMaterial()][sb2.getMaterial()].friction;
            double fNumerator = relVel.dot(tangent);
            fNumerator *= friction;
            double f = fNumerator / jDenom;

            // adjust velocity if relative velocity is moving toward each other.
            if (relDot <= 0.0001f)
            {
                if (A.isMoveable())
                {
                    //A->Velocity += ( info.norm * (j / A->Mass)) - (tangent * (f / A->Mass));
                    //A.getVelocity().add( info.norm.getScaled(j / A.mass).getSubbed(tangent.getScaled(f / A.mass)) );
                    helpVecB.become(tangent);
                    helpVecB.scale(f/A.mass);
                    helpVecA.become(info.norm);
                    helpVecA.scale(j/A.mass);
                    helpVecA.sub(helpVecB);
                    A.getVelocity().add(helpVecA);
                }

                if (b2MassSum != 0.0f)
                {
                    //B1->Velocity -= (info.norm * (j / b2MassSum) * b1inf) - (tangent * (f / b2MassSum) * b1inf);
                    //B1.getVelocity().sub( info.norm.getScaled(j / b2MassSum * b1inf).getSubbed(tangent.getScaled(f / b2MassSum * b1inf)) );
                    helpVecB.become(tangent);
                    helpVecB.scale(f / b2MassSum * b1inf);
                    helpVecA.become(info.norm);
                    helpVecA.scale(j/b2MassSum*b1inf);
                    helpVecA.sub(helpVecB);
                    B1.getVelocity().sub(helpVecA);
                }

                if (b2MassSum != 0.0f)
                {
                    //B2->Velocity -= (info.norm * (j / b2MassSum) * b2inf) - (tangent * (f / b2MassSum) * b2inf);
                    //B2.getVelocity().sub( info.norm.getScaled(j / b2MassSum * b2inf).getSubbed(tangent.getScaled(f / b2MassSum * b2inf)) );
                    helpVecB.become(tangent);
                    helpVecB.scale(f / b2MassSum * b2inf);
                    helpVecA.become(info.norm);
                    helpVecA.scale(j / b2MassSum * b2inf);
                    helpVecA.sub(helpVecB);
                    B2.getVelocity().sub(helpVecA);
                }
            }
        }
    }
}
